#include "task_planner/waypoints_config.hpp"
#include <filesystem>

namespace task_planner
{

bool WaypointsConfig::loadFromFile(const std::string& config_file_path)
{
  try
  {
    // 检查文件是否存在
    if (!std::filesystem::exists(config_file_path))
    {
      RCLCPP_ERROR(rclcpp::get_logger("WaypointsConfig"), 
                   "配置文件不存在: %s", config_file_path.c_str());
      return false;
    }
    
    // 加载YAML文件
    YAML::Node config = YAML::LoadFile(config_file_path);
    
    // 加载初始目标点
    if (config["initial_waypoints"])
    {
      initial_waypoints_.clear();
      for (const auto& waypoint_node : config["initial_waypoints"])
      {
        initial_waypoints_.push_back(parseWayPoint(waypoint_node));
      }
      RCLCPP_INFO(rclcpp::get_logger("WaypointsConfig"), 
                  "加载了 %zu 个初始目标点", initial_waypoints_.size());
    }
    
    // 加载C区过渡点
    if (config["c_zone_corner_points"])
    {
      c_zone_corner_points_.clear();
      for (const auto& corner : config["c_zone_corner_points"])
      {
        std::string name = corner.first.as<std::string>();
        c_zone_corner_points_[name] = parseWayPoint(corner.second);
      }
      RCLCPP_INFO(rclcpp::get_logger("WaypointsConfig"), 
                  "加载了 %zu 个C区过渡点", c_zone_corner_points_.size());
    }
    
    // 加载数字位置
    if (config["number_positions"])
    {
      number_positions_.clear();
      for (const auto& number_pos : config["number_positions"])
      {
        int32_t number = number_pos.first.as<int32_t>();
        number_positions_[number] = parseWayPoint(number_pos.second);
      }
      RCLCPP_INFO(rclcpp::get_logger("WaypointsConfig"), 
                  "加载了 %zu 个数字位置", number_positions_.size());
    }
    
    // 加载路径段配置
    if (config["path_segments"])
    {
      const auto& path_config = config["path_segments"];
      if (path_config["top_y"]) top_y_ = path_config["top_y"].as<double>();
      if (path_config["bottom_y"]) bottom_y_ = path_config["bottom_y"].as<double>();
      if (path_config["tolerance"]) tolerance_ = path_config["tolerance"].as<double>();
    }
    
    // 加载默认位置
    if (config["default_position"])
    {
      default_position_ = parseWayPoint(config["default_position"]);
    }
    
    RCLCPP_INFO(rclcpp::get_logger("WaypointsConfig"), 
                "配置文件加载成功: %s", config_file_path.c_str());
    return true;
  }
  catch (const std::exception& e)
  {
    RCLCPP_ERROR(rclcpp::get_logger("WaypointsConfig"), 
                 "加载配置文件失败: %s, 错误: %s", 
                 config_file_path.c_str(), e.what());
    return false;
  }
}

WayPoint WaypointsConfig::getNumberPosition(int32_t number) const
{
  auto it = number_positions_.find(number);
  if (it != number_positions_.end())
  {
    return it->second;
  }
  else
  {
    RCLCPP_WARN(rclcpp::get_logger("WaypointsConfig"), 
                "未找到数字 %d 的位置，使用默认位置", number);
    return default_position_;
  }
}

std::vector<WayPoint> WaypointsConfig::getCZoneCornerPointsArray() const
{
  std::vector<WayPoint> corner_points(6);
  
  // 按顺序：A, AB_mid, B, C, CD_mid, D
  const std::vector<std::string> corner_names = {
    "A", "AB_mid", "B", "C", "CD_mid", "D"
  };
  
  for (size_t i = 0; i < corner_names.size(); ++i)
  {
    auto it = c_zone_corner_points_.find(corner_names[i]);
    if (it != c_zone_corner_points_.end())
    {
      corner_points[i] = it->second;
    }
    else
    {
      RCLCPP_WARN(rclcpp::get_logger("WaypointsConfig"), 
                  "未找到过渡点: %s，使用默认位置", corner_names[i].c_str());
      corner_points[i] = default_position_;
    }
  }
  
  return corner_points;
}

WayPoint WaypointsConfig::parseWayPoint(const YAML::Node& node)
{
  WayPoint point;
  
  if (node["x"]) point.x = node["x"].as<double>();
  if (node["y"]) point.y = node["y"].as<double>();
  if (node["z"]) point.z = node["z"].as<double>();
  if (node["yaw"]) point.yaw = node["yaw"].as<double>();
  
  return point;
}

} // namespace task_planner
